#include "ms5803.h"
#include "math.h"
#include "softiic.h"
//extern I2C_HandleTypeDef hi2c1;
static uint16_t C1,C2,C3,C4,C5,C6;
static int      DT,TEMP,P;
static int64_t OFF,SENS,T2,OFF2,SENS2;

pfunc MS5803_Read_Data = NULL;
pfunc MS5803_Write_Data = NULL;
void   HAL_Wride_Data(uint8_t daddr,uint8_t addr,uint8_t *data,uint16_t len)
{
//    HAL_I2C_Master_Transmit(&hi2c1,daddr<<1,&addr,1,1000);
//    HAL_Delay(1);
//    if(len != 0)
//    HAL_I2C_Master_Transmit(&hi2c1,daddr<<1,data,len,1000);
}
void   HAL_Read_Data(uint8_t daddr,uint8_t addr,uint8_t *data,uint16_t len)
{
//    HAL_I2C_Master_Transmit(&hi2c1,daddr<<1,&addr,1,1000);
//    HAL_Delay(1);
//    if(len != 0)
//        HAL_I2C_Master_Receive(&hi2c1,daddr<<1,data,len,1000);
}

/******************************************************************************
 * Name:    CRC-4/ITU           x4+x+1
 * Poly:    0x03
 * Init:    0x00
 * Refin:   True
 * Refout:  True
 * Xorout:  0x00
 * Note:
 *****************************************************************************/
static uint8_t crc4_itu(uint8_t *data, uint8_t length)
{
    uint8_t i;
    uint8_t crc = 0;		// Initial value
    while(length--)
    {
        crc ^= *data++;	 	// crc ^= *data; data++;
        for (i = 0; i < 8; ++i)
        {
            if (crc & 1)
                crc = (crc >> 1) ^ 0x0C;// 0x0C = (reverse 0x03)>>(8-4)
            else
                crc = (crc >> 1);
        }
    }
    return crc;
}
void MS5803_Init()
{
    //set i2c interface
    //MS5803_Write_Data = HAL_Wride_Data;
    //MS5803_Read_Data  = HAL_Read_Data;
    IIC_Init();
    MS5803_Write_Data = SoftIIC_Write_Data;
    MS5803_Read_Data  = SoftIIC_Read_Data;

    //Send reset command
    MS5803_Write_Data(MS5803_ADDR,MS5803_RESET,NULL,0);
    HAL_Delay(100);
    //begin read adjust prom
    if(MS5803_Get_Adjust_Args() == 0)
    {
        //printf("read prom success\r\n");
    }
}

uint8_t MS5803_Get_Adjust_Args()
{
    uint8_t buf[16]={0};
    for(int i=0;i<8;i++)
    {
        MS5803_Read_Data(MS5803_ADDR,MS5803_RESERVED+i*2,&buf[i*2],2);
        HAL_Delay(10);
    }
    //begin do crc4 check
    uint8_t calCrc = crc4_itu(buf+2,13);
    uint8_t recCrc = buf[15];
    //if(calCrc == recCrc)
    {
        C1 = buf[2]<<8|buf[3];
        C2 = buf[4]<<8|buf[5];
        C3 = buf[6]<<8|buf[7];
        C4 = buf[8]<<8|buf[9];
        C5 = buf[10]<<8|buf[11];
        C6 = buf[12]<<8|buf[13];
        return 0;
    }
    return -1;
}
float MS5803_Get_Temperature()
{
    uint8_t buf[3]={0};
    MS5803_Write_Data(MS5803_ADDR,MS5803_CONV_TEMP,NULL,0);
    HAL_Delay(50);
    MS5803_Read_Data(MS5803_ADDR,MS5803_READ,buf,3);
    int result = buf[0]<<16|buf[1]<<8|buf[2];
    DT = result - (C5<<8);
    TEMP = 2000 + DT*(C6/pow(2,23));
    if(TEMP < 20)
    {
        T2 = 3*pow(DT,2)/pow(2,33);
    }
    else
    {
        T2 = 7*pow(DT,2)/pow(2,37);
    }
    TEMP -= T2;
    return (TEMP/100.0);
}
float MS5803_Get_Pressure()
{
    uint8_t buf[3]={0};
    MS5803_Write_Data(MS5803_ADDR,MS5803_CONV_PRESSURE,NULL,0);
    HAL_Delay(50);
    MS5803_Read_Data(MS5803_ADDR,MS5803_READ,buf,3);
    int result = buf[0]<<16|buf[1]<<8|buf[2];
    //cal off sens 
    SENS = (int64_t)(C1<<15)+ (int64_t)(DT>>8*C3);
    OFF =  (int64_t)(C2<<16) + (int64_t)(DT>>7*C4);
    if(TEMP < 2000)
    {
        OFF2 = 3*pow((TEMP-2000),2)/2;
        SENS2 = 5*pow((TEMP,2000),2)/8;
        if(TEMP < -15000)
        {
            OFF2 += 7*pow((TEMP+1500),2);
            SENS2 += 4*pow((TEMP+1500),2);
        }
    }
    else
    {
        OFF2 = pow((TEMP-2000),2)/16;
        SENS2 = 0;
    }
    OFF = OFF - OFF2;
    SENS = SENS - SENS2;
    P = ((int)(result*SENS>>21))>>15;
    P -= (OFF>>15);
    return (P/10.0);
}
void  MS5803_Test()
{
    uint8_t buf[3]={0};
    MS5803_Write_Data(MS5803_ADDR,MS5803_CONV_TEMP,NULL,0);
    HAL_Delay(50);
    MS5803_Read_Data(MS5803_ADDR,MS5803_READ,buf,3);
    int result = buf[0]<<16|buf[1]<<8|buf[2];
    DT = result - (C5<<8);
    printf("result %d\r\n",result);
}